#include "Hchassis.h"

double Hchassis::get_enc(int side, rotationUnits unit_r) {
  if (side == -1)
    return p_au_l->get_enc(0, unit_r);
  if (side == 1)
    return p_au_r->get_enc(0, unit_r);
  return 0;
}

double Hchassis::get_actual_velocity(int side, int mtr_index,
                                    velocityUnits units_v) {
  if (side == -1)
    return p_au_l->get_actual_velocity(mtr_index, units_v);
  if (side == 1)
    return p_au_r->get_actual_velocity(mtr_index, units_v);
  return 0;
}

void Hchassis::reset_enc() {
  p_au_l->reset_enc();
  p_au_r->reset_enc();
}

void Hchassis::set_stopping_without_saving(brakeType brake_type) {
  p_au_l->set_stopping(brake_type);
  p_au_r->set_stopping(brake_type);
}

void Hchassis::set_stopping(brakeType brake_type) {
  this->set_brake_type = brake_type;
  p_au_l->set_stopping(brake_type);
  p_au_r->set_stopping(brake_type);
}
char Hchassis::get_stopping_char() {
  if (ActionUnit::is_all_units_relaxing())
    return 'c';
  if (auto_hold_turned_on)
    return 'h';
  else {
    if (set_brake_type == brake)
      return 'k';
    if (set_brake_type == coast)
      return 'c';
    if (set_brake_type == hold)
      return 'h';
  }
  return 0;
}
void Hchassis::mtr_stop(brakeType brake_type) {
  p_au_l->mtr_stop(brake_type);
  p_au_r->mtr_stop(brake_type);
}

void Hchassis::stop() {
  p_au_l->stop();
  p_au_r->stop();
  forward_pid_adjust_force_stop = true;
}

/////////////////////////////////////////////////////////////////////
void Hchassis::LR_set_voltage(double mv_pct_l, double mv_pct_r) {
  p_au_l->mtr_set_voltage(mv_pct_l);
  p_au_r->mtr_set_voltage(mv_pct_r);
}

void Hchassis::LR_set_speed(double v_pct_l, double v_pct_r) {
  p_au_l->set_speed(v_pct_l);
  p_au_r->set_speed(v_pct_r);
}

void Hchassis::FT_set_voltage(double forward_pct, double turn_pct) {
  LR_set_voltage(forward_pct + turn_pct, forward_pct - turn_pct);
}
void Hchassis::FT_set_speed(double forward_pct, double turn_pct) {
  LR_set_speed(forward_pct + turn_pct, forward_pct - turn_pct);
}

/////////////////////////////////////////////////////////////////////////
double Hchassis::distance_to_rev(double distance, distanceUnits units) {
  ///////////////////////
  // distance = distance * 0.985; //参数修正，少跑

  double k;
  if (units == distanceUnits::cm)
    k = 10;
  else if (units == distanceUnits::in)
    k = 25.4;
  else
    k = 1;
  double dist = distance * k;
  double axis_rev = dist / _wheel_travel * _external_gear_reduction_ratio;
  return axis_rev;
}

double Hchassis::rev_to_distance_mm(double r) {

  double dist = r / _external_gear_reduction_ratio * _wheel_travel;
  // dist *= 0.985; //显示参数修正
  return dist;
}

double Hchassis::rev_to_raw(double r, gearSetting s) {
  float k;
  if (s == gearSetting::ratio36_1)
    k = 1800;
  if (s == gearSetting::ratio18_1)
    k = 900;
  if (s == gearSetting::ratio6_1)
    k = 300;
  double raw_enc = k * r;
  return raw_enc;
}

double Hchassis::get_dist_mm(int side, int mtr_index) {
  double dist;
  if (side == -1)
    dist = rev_to_distance_mm(p_au_l->get_enc(mtr_index, rotationUnits::rev));
  if (side == 1)
    dist = rev_to_distance_mm(p_au_r->get_enc(mtr_index, rotationUnits::rev));
  return dist;
}

double Hchassis::get_dist_mm_avg() {
  return (get_dist_mm(-1) + get_dist_mm(1)) / 2;
}